import Box2D as box2d

world = 0
cdef float pi = <float> 3.14159265
cdef float timeStep = <float> (1.0 / 60.0)
cdef int velocityIterations = 10
cdef int positionIterations = 8
contactlistener = 0
cdef float physScale = <float> 0.01
cdef float degtorad_constant = <float> (pi/180.0)
cdef float radtodeg_constant = <float> (180.0/pi)
delete_me = []
create_me = []

cdef scale_value(float value):
	"""
	Returns <float> value * physScale
	"""
	global physScale
	return <float> value * physScale

cdef unscale_value(float value):
	"""
	Returns <float> value / physScale
	"""
	global physScale
	return <float> value / physScale

cpdef scale(obj):
	"""
	Attempts to find all scalable values in obj and returns them scaled.
	
	If obj is a b2Vec2 then scale will return a b2Vec2 with it's x,y scaled.
	
	If obj is iterable, it will recursivly apply scale to each item.
	
	If obj is a value, it will return it converted to a long and scaled.
	"""
	try:
		return box2d.b2Vec2(scale_value(obj.x), scale_value(obj.y))
	except AttributeError:
		try:
			return map(scale, obj)
		except (IndexError, TypeError):
			return scale_value(obj)

def unscale(obj):
	"""
	Attempts to find all scalable values in obj and returns them unscaled.
	
	If obj is a b2Vec2 then scale will return a b2Vec2 with it's x,y unscaled.
	
	If obj is iterable, it will recursivly apply unscale to each item.
	
	If obj is a value, it will return it converted to a long and unscaled.
	"""
	try:
		return (unscale_value(obj.x), unscale_value(obj.y))
	except AttributeError:
		try:
			return map(unscale, obj)
		except (IndexError, TypeError):
			return unscale_value(obj)

cpdef create_body(bodydef):
	"""
	Internal function, rename for world.CreateBody()
	
	Probably shouldn't exist.
	"""
	return world.CreateBody(bodydef)

cpdef clean_up():
	"""
	Internal function.
	
	Calling this calls obj.delete() for obj in delete_me[]
	
	Since box2d entities can't always be deleted when they want to be,
	 it's more reasonable to queue deletions and process them when it's safe.
	
	Call this after each world timestep to be as accurate as possible.
	
	Debatably, this process produces inaccuracies in the physics sim, if
	 the user expects the entity to be removed instantly and it is not.
	This occurs most when the entity is involved in multiple collisions
	 during a single timestep and the first of which asks the entity to
	 die.
	
	The best AFAIK resolution for this is for shape code to take whatever
	 action it can to prevent the entity from colliding further.
	"""
	global delete_me
	for obj in delete_me:
		obj.delete()
	delete_me = []

cpdef time_step():
	"""
	Public function.
	
	Call this whenever you want the timestep to occur. (just before drawing)
	
	TBD:
	 - Add optional parameters for step length, velocity and position iters
	 - Add optional parameter for world.Validate()
	"""
	#genesis()
	world.time_step(timeStep, velocityIterations, positionIterations)
	#world.Validate()
	#contactlistener.pulse()
	#clean_up()

def init(width, height, gravity=(0,0), sleep=True):
	"""
	Public function.
	
	Initializes tygger, must be called before using and once per instance.
	
	width and height are the size of the world, unscaled.
	Generally this is twice the screen size.
	
	gravity is a tuple or a vector with x,y properties.
	Passing (0,-9.81) will set gravity to be earthlike.
	Default is (0,0) (no gravity)
	
	sleep is boolean, should always be True.
	If False, objects at rest will still be processed.
	"""
	global world
	#global contactlistener
		
	world = World(width, height, gravity, sleep)
	
	#contactlistener = PhysicsCollisionNotifer()
	#destructionlistener = 
	
	#world.SetContactListener(contactlistener)
	#world.SetDestructionListener(destructionlistener)

class World(object):
	"""
	Container for b2World.
	
	The world is where all bodies, shapes, and joints live.
	"""
	def __init__(self, width, height, gravity=(0,0), sleep=True):
		"""
		Public Function.
		
		Return a world object for storing tygger entities.
		
		@type  width: number
		@param width: The width of the world, typically twice the screen width
		@type  height: number
		@param height: The height of the world, typically twice the screen height.
		@type  gravity: tuple or vector with x,y
		@param gravity: The world's gravity vector. (0,-9.81) is earth like gravity, (0,0), the defalt, is no gravity.
		@type  sleep: bool
		@param sleep: This should always be True. Remove this parameter after tygger is stable.
		@rtype : tygger.World
		@return: A useable world object.
		"""
		worldAABB = box2d.b2AABB()
		worldAABB.lowerBound.Set(scale(-(width*2)), scale(-(height*2)))
		worldAABB.upperBound.Set(scale((width*2)), scale((height*2)))
		
		try:
			gravityb2v = box2d.b2Vec2(gravity[0],gravity[1])
		except TypeError:
			gravityb2v = box2d.b2Vec2(gravity.x,gravity.y)
		
		self.phys_world = box2d.b2World(worldAABB, gravityb2v, sleep)
		
		self.garbage_bin = []
		
		self.phys_contactlistener = PhysicsCollisionNotifer()
		#self.phys_world.SetContactListener(self.phys_contactlistener)
	
	def time_step(self, timeStep, velocityIterations, positionIterations):
		self.phys_world.Step(timeStep, velocityIterations, positionIterations)
		self.phys_world.Validate()
		self.take_out_the_trash()
	
	def create_body(self, phys_bodydef):
		"""
		Creates the PhysicsBody passed to it in the world.
		
		@type  physbody: PhysicsBody
		@param physbody: The PhysicsBody or subclass to be created.
		@rtype : b2Body
		@return: A Box2D body representing the body created. 
		"""
		return self.phys_world.CreateBody(phys_bodydef)
	
	def create_joint(self, phys_jointDef):
		"""
		Creates the joint passed to it in the world.
		
		@type  physjoint: *Joint class
		@param physjoint: The joint to be created
		@rtype  : Box2D joint class matching type of joint to be created.
		@return : Box2D joint class matching type of joint to be created.
		"""
		return self.phys_world.CreateJoint(phys_jointDef).getAsType()
	
	def get_static_body(self):
		"""
		Returns a body useable for static shapes.
		
		@rtype  : b2Body
		@return : Shared static b2Body for attaching static shapes.
		"""
		return self.phys_world.GetGroundBody()
	
	def mark_for_deletion(self, physics):
	#	self.garbage_bin.append.(physics)
		pass
	
	def take_out_the_trash(self):
		for trash in self.garbage_bin:
			trash.delete()
		self.garbage_bin = []
	

class DistanceJoint(object):
	"""
	Container for Distance Joints.
	
	A distance joint preseves the distance between two bodies.
	"""
	def __init__(self, physbody1, physbody2):
		"""
		Public Function.
		
		physbody1 and physbody2 are the two 'PhysicsBody's involved.
		
		This init assumes the bodies are properly placed.
		"""
		global world
		self.phys_jointDef = box2d.b2DistanceJointDef()
		self.phys_jointDef.Initialize(physbody1.phys_body, physbody2.phys_body, physbody1.phys_body.GetWorldCenter(), physbody2.phys_body.GetWorldCenter())
		self.phys_jointDef.collideConnected = True
		self.phys_joint = world.create_joint(self.phys_jointDef).getAsType()
	
	def remove(self):
		"""
		Public Function.
		
		Call this to delete the joint.
		"""
		#cleanup stage one
		global delete_me
		delete_me.append(self)
	
	def delete(self):
		"""
		Internal function.
		
		Called when deletes are processed by tygger.cleanup()
		"""
		#cleanup stage two
		global world
		world.DestroyJoint(self.phys_joint)
		self.phys_joint = None
	

class RevoluteJoint(object):
	"""
	Container for Revolute Joints.
	
	A revolute joint anchors the two objects to a common point.
	"""
	def __init__(self, physbody1, physbody2, lower_limit='False', upper_limit='False', max_motor_torque = 0.0, motor_speed = 0.0, motor_enabled=False):
		"""
		Public Function
		
		physbody1 and physbody2 are the two bodies involved.
		
		lower_limit and upper_limit are two angles in degrees that
		 limit the joint to rotate in thier range.
		Zero should be included in this range, or the joint will
		 'jump' to a valid angle during the simulation.
		
		motor_speed, max_motor_torque and motor_enabled all deal
		 with joint the joint's motor, which if motor_enabled is True,
		 will rotate the joint accelerating at motor_speed, until
		 max_motor_torque is achived.
		"""
		global world
		self.phys_jointDef = box2d.b2RevoluteJointDef()
		self.phys_jointDef.Initialize(physbody1.phys_body, physbody2.phys_body, physbody1.phys_body.GetWorldCenter())
		if(lower_limit != 'False' and upper_limit != 'False'):
			self.phys_jointDef.lowerAngle = lower_limit * degtorad_constant
			self.phys_jointDef.upperAngle = upper_limit * degtorad_constant
			self.phys_jointDef.enableLimit = True
		if(motor_enabled):
			self.phys_jointDef.maxMotorTorque = max_motor_torque
			self.phys_jointDef.motorSpeed = motor_speed * pi
			self.phys_jointDef.enableMotor = True
		self.phys_joint = world.create_joint(self.phys_jointDef).getAsType()
	
	def get_joint_angle(self):
		"""
		Public Function.
		
		Returns curent joint angle in degrees.
		"""
		return self.get_joint_radian() * (180.0/pi)
	
	def get_joint_radian(self):
		"""
		Public Function.
		
		Returns current joint angle in radians.
		"""
		return self.phys_joint.GetJointAngle()
	
	def update_speed(self, speed):
		"""
		Public Function.
		
		Sets the joint motor's speed to speed, a float.
		"""
		self.phys_joint.SetMotorSpeed(speed * pi)
	
	def set_target_angle(self, angle, gain=2):
		"""
		Public Function.
		
		Should set the track the target angle given in degrees at gain.
		Doesn't do anything right though.
		"""
		self.set_target_radian((angle * degtorad_constant), gain * degtorad_constant)
	
	def set_target_radian(self, radian, gain=(2*degtorad_constant)):
		"""
		Public Funtion.
		
		Should set the joint to track the target angle given in degrees at gain.
		Doesn't work.
		"""
		curr_angle = self.get_joint_radian()
		if(curr_angle < (radian - gain)):
			speed = (abs(radian-curr_angle)**2)*2
			if(speed >= 1):
				self.update_speed(speed)
			else:
				self.update_speed(1)
		elif(curr_angle > (radian + gain)):
			speed = -((abs(radian-curr_angle)**2)*2)
			if(speed <= -1):
				self.update_speed(speed)
			else:
				self.update_speed(-1)
		else:
			self.update_speed(0)
	
	def remove(self):
		"""
		Public Function.
		
		Stage 1 removal, will set the joint to be removed during cleanup()
		"""
		#cleanup stage one
		global delete_me
		delete_me.append(self)
	
	def delete(self):
		"""
		Private Function.
		
		Stage 2 removal, will actually delete world.
		"""
		#cleanup stage two
		global world
		world.DestroyJoint(self.phys_joint)
		self.phys_joint = None
	

class SpringJoint(object):
	"""
	Container for Box2d mouse joint.
	
	Once created, call set_target() with the new mouse corordinates and 
	 it will update the mouse joint's target.
	"""
	def __init__(self, phys_body, x, y, speed):
		"""
		Public Function.
		
		phys_body is the PhysicsBody being mousejointed.
		
		x, y is the first target of the joint.
		
		speed is the constant rate at which the phys_body will be pulled.
		It is mass independant, a speed of 3000 will move a larger
		 object at the same rate as a smaller one.
		"""
		global world
		self.phys_body = phys_body
		self.phys_jointDef = box2d.b2MouseJointDef()
		self.phys_jointDef.body1 = phys_body
		self.phys_jointDef.body2 = phys_body
		pos = phys_body.GetPosition()
		self.phys_jointDef.target.Set(pos.x, pos.y)
		self.phys_jointDef.maxForce = speed * phys_body.GetMass()
		self.phys_joint = world.create_joint(self.phys_jointDef).getAsType()
		self.phys_joint.SetTarget(scale(box2d.b2Vec2(x,y)))
		self.phys_body.WakeUp()
	
	def set_target(self, x,y):
		"""
		Public Function.
		
		Updates the joint's target.
		"""
		self.phys_joint.SetTarget(scale(box2d.b2Vec2(x,y)))
		self.phys_body.WakeUp()
	
	def remove(self):
		"""
		Public Function.
		
		Removal stage 1, call this and the joint will be removed
		 at the next oppurtunity.
		"""
		#cleanup stage one
		global delete_me
		delete_me.append(self)
	
	def delete(self):
		"""
		Private Function.
		
		Called during cleanup() to remove the joint.
		"""
		#cleanup stage two
		global world
		world.DestroyJoint(self.phys_joint)
		self.phys_joint = None
	

class PhysicsBody(object):
	"""
	Container class for a Box2D Body.
	
	A body holds the following non-collision related information:
	 - Where the body is.
	 - Is the body Continously Collision Detected (CCD)
	 - 
	"""
	def __init__(self, x=0, y=0, CCD=False, fixed_rotation=False, allow_sleep=True, b2body=None):
		if(b2body):
			self.phys_body = b2body
			self.phys_is_ground = True
		else:
			self.phys_bodydef = box2d.b2BodyDef()
			self.phys_bodydef.position.Set(scale(x),scale(y))
			self.phys_bodydef.isBullet = CCD
			self.phys_bodydef.fixedRotation=fixed_rotation
			self.phys_bodydef.allowSleep = allow_sleep
			self.phys_body = world.create_body(self.phys_bodydef)
			self.phys_is_ground = False
		self.phys_targetJoint = None
	
	def get_pos(self):
		return unscale(self.phys_body.GetPosition())
	
	def get_angle(self):
		"""
		Returns the current angle of the body in degrees.
		"""
		return self.phys_body.GetAngle() * radtodeg_constant * -1
	
	def get_location(self):
		"""
		Returns a three value tuple in the form of (x, y, rotation)
		 where x, y are the position of the body,
		 and rotation is the angle of rotation in degrees
		"""
		x, y = self.get_pos()
		return (x, y, self.get_angle())
	
	def remove(self):
		#cleanup stage one
		global delete_me
		delete_me.append(self)
	
	def delete(self):
		#cleanup stage two
		global world
		world.DestroyBody(self.phys_body)
		self.phys_body = None
	
class PhysicsGeometeryError(Exception):
	def __init__(self, value):
		self.value = value
	
	def __str__(self):
		return repr(self.value)
	

class PhysicsGeometry(object):
	def __init__(self, x=0, y=0, w=0, h=0, r=0, vertex_list=[], density=1, friction=0.3, collide_func=None):
		self.phys_center = (scale(x), scale(y))
		
		if(w > 0 and h > 0):
			self.phys_radius = 0
			self.phys_width = scale(w)
			self.phys_height = scale(h)
			self.phys_shapedef = box2d.b2PolygonDef()
			self.phys_shapedef.SetAsBox(self.phys_height/2., self.phys_height/2.)
		elif(r > 0):
			self.phys_radius = scale(r)
			self.phys_shapedef = box2d.b2CircleDef()
			self.phys_shapedef.radius = self.phys_radius
			self.phys_width = self.phys_radius
			self.phys_height = self.phys_radius
		elif(len(vertex_list) > 0):
			self.phys_radius = 0
			self.phys_width = 0
			self.phys_height = 0
			self.phys_vertex_list = scale(vertex_list)
			self.phys_shapedef = box2d.b2EdgeChainDef()
			self.phys_shapedef.setVertices_tuple(self.phys_vertex_list)
		else:
			raise PhysicsGeometeryError("No valid geometery specified")
		self.phys_shapedef.density = density
		self.phys_shapedef.friction = friction
		self.phys_shapedef.userData = self
		if(collide_func):
			self.collide = collide_func
	
	def collide(contact_point):
		pass
	
	def attach_to_body(self, physbody):
		if (self.phys_radius > 0):
			point = physbody.phys_body.GetLocalPoint(box2d.b2Vec2(self.phys_center[0], self.phys_center[1]))
			self.phys_shapedef.localPosition.Set(point.x, point.y)
		elif(self.phys_width > 0 and self.phys_height > 0):
			adjusted_verts = []
			for vertex in self.phys_shapedef.vertices:
				adjusted_verts.append((vertex[0] + self.phys_center[0], vertex[1] + self.phys_center[1]))
			self.phys_shapedef.setVertices_tuple(adjusted_verts)
		
		self.phys_shape = physbody.phys_body.CreateShape(self.phys_shapedef).getAsType()
	
	def remove(self):
		world.mark_for_deletion(self)
	
	def delete(self):
		#cleanup stage two
		global world
		try:
			self.phys_body.DestroyShape(self.phys_shape)
		except AttributeError:
			pass
		self.phys_shape = None
		del self.phys_shape
	

class PhysicsStaticCollisionBody(PhysicsBody, PhysicsGeometry):
	def __init__(self, x=0, y=0, r=0, w=0, h=0, density=1, friction=0.3,
	CCD=False, fixed_rotation=False, allow_sleep=True, collide_func=None, b2body=None):
		PhysicsBody.__init__(self, x=x, y=y, CCD=CCD, fixed_rotation=fixed_rotation, allow_sleep=allow_sleep)
		PhysicsGeometry.__init__(self, x=x, y=y, w=w, h=h, r=r, density=density, friction=friction,
		collide_func=collide_func)
		self.attach_to_body(self)
	



class PhysicsCollisionBody(PhysicsBody, PhysicsGeometry):
	def __init__(self, x=0, y=0, r=0, w=0, h=0, density=1, friction=0.3,
	CCD=False, fixed_rotation=False, allow_sleep=True, collide_func=None, b2body=None):
		PhysicsBody.__init__(self, x=x, y=y, CCD=CCD, fixed_rotation=fixed_rotation, allow_sleep=allow_sleep)
		PhysicsGeometry.__init__(self, x=x, y=y, w=w, h=h, r=r, density=density, friction=friction,
		collide_func=collide_func)
		self.attach_to_body(self)
		self.phys_body.SetMassFromShapes()
	
class PhysicsStaticCollisionEdge(PhysicsBody, PhysicsGeometry):
	def __init__(self, vertex_list=[], density=1, friction=0.3,
	CCD=False, fixed_rotation=False, allow_sleep=True, collide_func=None, b2body=None):
		PhysicsBody.__init__(self, b2body=world.get_static_body())
		PhysicsGeometry.__init__(self, vertex_list=vertex_list, density=density, friction=friction,
		collide_func=collide_func)
		self.attach_to_body(self)

class PhysicsCollisionDetails(object):
	shape1 = None
	shape2 = None
	normal = None
	position = None
	velocity = None
	id  = None


class PhysicsCollisionNotifer(box2d.b2ContactListener):
	def __init__(self):
		super(PhysicsCollisionNotifer, self).__init__()
		self.collisions = []
	
	def pulse(self):
		for collision in self.collisions:
			try:
				collision.this.collide(collision)
			except AttributeError:
				pass
		self.collisions = []
	
	def Add(self, point):
		"""Handle add point"""
		cp = PhysicsCollisionDetails()
		cp.this = point.shape1.GetUserData()
		cp.other = point.shape2.GetUserData()
		cp.position = point.position.copy()
		cp.normal = point.normal.copy()
		cp.id = point.id.key
		self.collisions.append(cp)
		cp.other, cp.this = cp.this, cp.other
		cp.normal.x = cp.normal.x * -1
		cp.normal.y = cp.normal.y * -1
		self.collisions.append(cp)
	
	def Persist(self, point):
		"""Handle persist point"""
		return
		cp = PhysicsCollisionDetails()
		cp.this = point.shape1
		cp.other = point.shape2
		cp.position = point.position.copy()
		cp.normal = point.normal.copy()
		cp.id = point.id.key
		if cp.this.GetUserData() != ['ground']:
			self.collisions.append(cp.this.GetUserData())
			cp.this.GetUserData().new_contact(cp)
		cp.other, cp.this = cp.this, cp.other
		if cp.this.GetUserData() != ['ground']:
			cp.normal.x = cp.normal.x * -1
			cp.normal.y = cp.normal.y * -1
			self.collisions.append(cp.this.GetUserData())
			cp.this.GetUserData().new_contact(cp)
	
	def Remove(self, point):
		"""Handle remove point"""
		pass
	

class PhysicsChain(object):
	#asumes everthing is already in the correct place
	def __init__(self, physbodylist):
		self.joints = []
		for i in range(len(physbodylist)-1):
			self.joints.append(RevoluteJoint(physbodylist[i], physbodylist[i+1]))
	


	

